#ifndef FILTER_H
#define FILTER_H


typedef struct {
    int buffer[10];                 //窗口宽度
    unsigned char ptr;              //下一个窗口位置
    int input;                      //输入数据
    int output;                     //滤波后数据
} window_filter_struct;


/**
  * @brief          初始化窗口滤波
  * @param[in,out]  *p: 结构体指针
  * @retval         无
  */
extern void Window_Filter_Init(window_filter_struct *p);


/**
  * @brief          计算窗口滤波
  * @param[in,out]  *p: 结构体指针
  * @retval         无
  */
extern void Window_Filter_Calc(window_filter_struct *p);

typedef struct
{
	float Angle;                    //模型估计值
	float Gyro_y;                   //
	float Q_angle;                  //过程噪声
	float Q_gyro;                   //
	float R_angle;                  //测量噪声
	float dt;	                    //
	char  C_0;                      //
	float Q_bias, Angle_err;        //
	float PCt_0, PCt_1, E;          //
	float K_0, K_1, t_0, t_1;       //卡尔曼增益
	float Pdot[4];                  //
	// float PP[2][2];			
	float PP;					    //误差协方差
} KALMAN_STRUCT;

extern void kalman_Init(KALMAN_STRUCT *p, float R_angle, float Q_angle);

extern void Kalman_Filter(KALMAN_STRUCT *p, float input);
#endif //FILTER_H

